#include "comms.h"

namespace comms {

const cAngles cAngles::Zero(0.0f, 0.0f, 0.0f);

//-----------------------------------------------------------------------------
// cAngles::Normalize360
//-----------------------------------------------------------------------------
void cAngles::Normalize360() {
	int i;

	for(i = 0; i < 3; i++) {
		if((*this)[i] >= 360.0f || (*this)[i] < 0.0f) {
			(*this)[i] -= cMath::Floor((*this)[i] / 360.0f) * 360.0f;
			if((*this)[i] >= 360.0f) {
				(*this)[i] -= 360.0f;
			}
			if((*this)[i] < 0.0f) {
				(*this)[i] += 360.0f;
			}
		}
	}
} // cAngles::Normalize360

//-----------------------------------------------------------------------------
// cAngles::Normalize180
//-----------------------------------------------------------------------------
void cAngles::Normalize180() {
	Normalize360();
	if(Pitch > 180.0f) {
		Pitch -= 360.0f;
	}
	if(Yaw > 180.0f) {
		Yaw -= 360.0f;
	}
	if(Roll > 180.0f) {
		Roll -= 360.0f;
	}
}


// cAngles::ToQuat
const cQuat cAngles::ToQuat() const {
	return ToMat3().ToQuat();
}

// cAngles::ToMat3
const cMat3 cAngles::ToMat3() const {
	return cMat3::RotationXYZ(Pitch, Yaw, Roll);
}

// cAngles::ToMat4
const cMat4 cAngles::ToMat4() const {
	return ToMat3().ToMat4();
}

// cAngles::ToVectors
void cAngles::ToVectors(cVec3 *Forward, cVec3 *Right, cVec3 *Up) const {
	ToMat3().ToVectors(Forward, Right, Up);
}

// cAngles::ToForward
const cVec3 cAngles::ToForward() const {
	return ToMat3().ToForward();
}

// cAngles::EnsureShortestPath
void cAngles::EnsureShortestPath(cAngles *l, cAngles *r) {
	cAssert(l != NULL);
	cAssert(r != NULL);

	l->Normalize180();
	r->Normalize180();
	r->Pitch = l->Pitch + cMath::AngleNormalize180(r->Pitch - l->Pitch);
	r->Yaw = l->Yaw + cMath::AngleNormalize180(r->Yaw - l->Yaw);
	r->Roll = l->Roll + cMath::AngleNormalize180(r->Roll - l->Roll);
}

} // comms